#ifndef _SMATRIX_H
#define _SMATRIX_H

#include <cstdlib>
#include <map>
#include <vector>
#include "Types.hpp"
#include <iostream>
#include <fstream>

class sMatrix
{
public:
    typedef std::map<size_t, std::map<size_t , real> > mat_t;
    typedef  mat_t::iterator row_iter;
    typedef  mat_t::const_iterator row_iter_const;
    typedef std::map<size_t, real> col_t;
    typedef col_t::iterator col_iter;
    typedef col_t::const_iterator col_iter_const;

    sMatrix(size_t i){ m=i; n=i; }
    sMatrix(size_t i, size_t j){ m=i; n=j; }

    inline real& operator()(size_t i, size_t j)
    {
        if(i>=m || j>=n) throw;
        return mat[i][j];
    }

    inline real operator()(size_t i, size_t j) const
    {
        if(i>=m || j>=n) throw;
        real k = mat.find(i)->second.find(j)->second;
        return k;
    }

    inline size_t getNVertex() const{ return m;}

    size_t getNElements() const{
        real NElements =0;
        for(size_t ii=0; ii< m; ++ii)
            NElements += mat.find(ii)->second.size()+1;
        return NElements;
    }

    inline size_t getiRowLength(size_t i) const { return mat.find(i)->second.size();}

    void addElement(size_t vertexId, size_t neighbourID, real value)
    {
        mat.find(vertexId)->second.insert(std::pair<size_t,real>(neighbourID,value));
    }

    void addRow(size_t vertexId)
    {
        sMatrix::row_iter its = mat.find(vertexId), end_its= mat.find(vertexId);
        if(its==end_its)
        {
            sMatrix::col_t a;
            mat.insert(std::pair<size_t,sMatrix::col_t>(vertexId,a));
        }
        else
        {
            std::cerr << std::endl;
            std::cerr << "WARNING: Two or more vertex have the same index: " << vertexId << std::endl;
            std::cerr << std::endl;
        }
    }
    //Computes f=sMatrix*u (Sparse Matrix- Vector Product) and returns the vector
    std::vector<real> operator*(const std::vector<real>& u)
    {
        if(this->m != u.size()) throw;
        std::vector<real> f(this->m);
        real sum;
        row_iter ii;
        col_iter jj;

        for(ii=this->mat.begin(); ii!=this->mat.end(); ++ii){
            sum=0;
            for(jj=(*ii).second.begin(); jj!=(*ii).second.end(); ++jj){
                sum += (*jj).second * u[(*jj).first];
            }
            f[(*ii).first]=sum;
        }
        return f;
    }

    inline const mat_t& sparseM() const {return mat;}

    friend std::ostream& printMat(std::ostream &out, sMatrix & mat);


protected:
    sMatrix(){}

private:

    mat_t mat;
    size_t m;
    size_t n;
};

std::ostream& printMat(std::ostream &out, sMatrix & mat)
{
    sMatrix::row_iter ii;
    sMatrix::col_iter jj;
    for(ii=mat.mat.begin(); ii!=mat.mat.end(); ii++){
        for( jj=(*ii).second.begin(); jj!=(*ii).second.end(); jj++){
            out << (*ii).first << ' ';
            out << (*jj).first << ' ';
            out << (*jj).second << std::endl;
        }
    } out << std::endl;
    return out;
}


real dotProduct(const std::vector<real> &u,const std::vector<real> &v )
{
    real sum=0.0;
    for (size_t i=0; i<u.size(); ++i)
        sum+=u[i]*v[i];
    return sum;
}
std::vector<real> vecMinus(const std::vector<real> &u,const std::vector<real> &v )
{
    std::vector<real> diff;
    for (size_t i=0; i<u.size(); ++i)
        diff.push_back(u[i]-v[i]);
    return diff;
}
std::vector<real> scalarMult(const std::vector<real> &u,const real &scalar )
{
    std::vector<real> mult;
    for (size_t i=0; i<u.size(); ++i)
        mult.push_back(u[i]*scalar);
    return mult;
}

void vecNormalize(std::vector<real> &u)
{
    real u_sum=0.0;
    for (size_t i=0; i<u.size(); ++i)
        u_sum+=u[i]*u[i];
    u_sum = 1.0/std::sqrt(u_sum);
    for (size_t i=0; i<u.size(); ++i)
        u[i]=u[i]*u_sum;
}

std::vector<real> addVec(const std::vector<real> &u,const std::vector<real> &v )
{
    std::vector<real> diff;
    for (size_t i=0; i<u.size(); ++i)
        diff.push_back(u[i]+v[i]);
    return diff;
}

std::vector<real> CGsolver(sMatrix& A, const std::vector<real>& b,const real& eps, const size_t& iterMax)
{
    size_t N_vertex=A.getNVertex();
    std::vector<real> x(N_vertex,0.0),r1=b,r2=b,tmp=b;
    size_t counter=1;
    std::vector<real> p,Ap;
    while(sqrt(dotProduct(r2,r2))>=eps*sqrt(dotProduct(b,b)) && counter<=iterMax)
    {
        if(counter==1)
            p=r1;
        else
            p=addVec(r2,scalarMult(p,dotProduct(r2,r2)/dotProduct(r1,r1)));
        Ap=A*p;
        real alpha=dotProduct(r2,r2)/dotProduct(p,Ap);
        x=addVec(x,scalarMult(p,alpha));
        r1=r2;
        r2=vecMinus(r1,scalarMult(Ap,alpha));
        ++counter;
        //for(size_t i=0;i<N_vertex;++i)
        //std::cout<<x[i]<<"  ";
        //std::cout<<"\n";
    }
    std::cout << "Error in previous CG Convergence is " << sqrt(dotProduct(r2,r2)) <<std::endl;
    return x;
}
real boundary1(real x, real y) { real r = sqrt(x*x+y*y); real theta = atan2(y,x); return  pow(r,2./3.)*sin(2./3.*theta);	}

std::vector<real> GSSolver(sMatrix& Stiff,const real& eps, const size_t& iterMax, std::map<size_t,Vertex> &vertex)
{

    // Set the Boundary Values in u
    // loop through the vertices
    std::vector<real> u, f;
    //size_t N_Vertex = Stiff.getNVertex();
    //size_t N_elements =Stiff.getNElements();

    u.push_back(0); // Singular Point
    f.push_back(0);
    for (size_t it = 1 ; it <vertex.size(); ++it)
    {
        if (vertex[it].isBoundary()) {
            real x = vertex[it].getCoordinate(0);
            real y = vertex[it].getCoordinate(1);
            u.push_back(boundary1(x,y));
        }

        else
            u.push_back(1.0); // Set the initial values of the Internal Nodes to be 1.
        f.push_back(0.0);
    }


    //int k;
    //std::cin >>k;
    // Do the GS iterator only for the elements which are Not on boundary
    for (size_t k = 1; k<iterMax; ++k)
    {
        for (auto ii=Stiff.sparseM().begin(); ii!=Stiff.sparseM().end(); ++ii)
        {
            real sum = 0.0;
            size_t vertexID = (*ii).first;

            auto it = vertex.find(vertexID);
            if (!it->second.isBoundary())
            {
               // std::cout << vertexID << " \n" ;
                 for (auto jj=(*ii).second.begin(); jj!=(*ii).second.end(); ++jj)
                {
                     size_t vertex2 = jj->first;
                 //   std::cout << vertex2 << " " ;

                    sum += Stiff(vertexID,vertex2)*u[vertex2];

                 }
                //std::cout << sum <<"\n";
               //int k;
               //std::cin >>k;
                sum =sum- Stiff(vertexID,vertexID)*u[vertexID];
                u[vertexID]=(f[vertexID] -sum)/Stiff(vertexID,vertexID);
            }

        }
       // std::cout << std::endl;
      //std::vector<real> g = vecMinus(f,Stiff*u);
      //  real error=dotProduct(g,g);
      //  real err= sqrt(error/N_elements);
     //   if (err < eps) {
      //      std::cout<<"Iterations "<<k<<"\n";
     //       break;
     //   }
    }
    return u;
}



struct Edge{

    Edge(std::pair<size_t,size_t> a)
    {
        edge =a;
        vertex1 = a.first;
        vertex2 = a.second;
    }
    size_t vertex1, vertex2;
    std::pair<size_t,size_t> edge;
};


class VertexSetHashFunction {
public:
    size_t operator ()(const Edge &hasher) const{ return hasher.vertex1+hasher.vertex2;}
};

class SetEqual {
public:
    bool operator ()(const Edge &hasherA, const Edge &hasherB) const
    {
        return ((hasherA.vertex1 ==hasherB.vertex1 && hasherA.vertex2 ==hasherB.vertex2)||(hasherA.vertex2 ==hasherB.vertex1 && hasherA.vertex1 ==hasherB.vertex2));
    }
};





#endif  /* _MATRIX_H */
